#include "rclcpp/rclcpp.hpp"
#include "rclcpp/wait_for_message.hpp"
#include "std_msgs/msg/int32.hpp"
#include <chrono>
#include <iostream>
#include <thread>

using namespace std::chrono_literals;

class SubscriptionNode : public rclcpp::Node {
public:
  SubscriptionNode(std::string name) : Node(name) {}
  void start() {
    while (true) {
      std::this_thread::sleep_for(500ms);
      auto message = std_msgs::msg::Int32();
      if (!rclcpp::wait_for_message(message, this->shared_from_this(),
                                    "topic_queue", 2s)) {
        RCLCPP_ERROR(this->get_logger(), "wait for message failed...");
        // } else {
        //   if (!rclcpp::wait_for_message(message, this->shared_from_this(),
        //                                 "topic_queue")) {
        // RCLCPP_ERROR(this->get_logger(), "wait for message failed...");
      } else {
        RCLCPP_INFO(this->get_logger(), "get number %d", message.data);
      }
    }
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SubscriptionNode>("case_wait_for_message");
  node->start();
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}